/*
 -------------------------------------------------------------------------------
 ServoGyro.cpp
 ServoGyro Project
 
 Initially created by Daniel Pickem on 1/20/12.
 
 Version 1.0
 -------------------------------------------------------------------------------
 */

//------------------------------------------------------------------------------
// Includes
//------------------------------------------------------------------------------
#include "ServoGyro.h"
#include <Servo.h>
#include <Arduino.h>

//------------------------------------------------------------------------------
// Friends
//------------------------------------------------------------------------------


//------------------------------------------------------------------------------
// Friend Overloaded Operators
//------------------------------------------------------------------------------


//------------------------------------------------------------------------------
// Lifecycle
//------------------------------------------------------------------------------

// Constructors
ServoGyro::ServoGyro()
:
_minAngle(0),
_maxAngle(180),
_minVoltage(0),
_maxVoltage(0),
_minPulseWidth(0),
_maxPulseWidth(0),
_refAngle(0),
_isCalibrated(0) {

}

ServoGyro::ServoGyro(int angleRange)
:
_minAngle(0),
_maxAngle(angleRange), 
_minVoltage(0),
_maxVoltage(0),
_minPulseWidth(0),
_maxPulseWidth(0),
_refAngle(0),
_isCalibrated(0) {

}

ServoGyro::ServoGyro(int minAngle, int maxAngle)
:
_minAngle(minAngle),
_maxAngle(maxAngle),
_minVoltage(0),
_maxVoltage(0),
_minPulseWidth(0),
_maxPulseWidth(0),
_refAngle(0),
_isCalibrated(0) {

}

// Destructor
ServoGyro::~ServoGyro() {
	
}

// Copy constructor
ServoGyro::ServoGyro(const ServoGyro& srcObj) {
	copyHelper(srcObj);
}

// Assignment operator
const ServoGyro& ServoGyro::operator=(const ServoGyro& rhsObj) {
	// Self-assignment check
	if (this == &rhsObj) {
		return (*this);
	}
	
	// Free old memory
	
	// Copy new memory
	copyHelper(rhsObj);
	
	return (*this);
}

// Copy helper function
void ServoGyro::copyHelper(const ServoGyro& srcObj) {
	_minAngle			= srcObj._minAngle;
	_maxAngle			= srcObj._maxAngle;
	_minVoltage			= srcObj._minVoltage;
	_maxVoltage 			= srcObj._maxVoltage;
	_minPulseWidth			= srcObj._minPulseWidth;
	_maxPulseWidth			= srcObj._maxPulseWidth;
	_refAngle 			= srcObj._refAngle;
	_isCalibrated			= srcObj._isCalibrated;
}

//------------------------------------------------------------------------------
// Overloaded Operators
//------------------------------------------------------------------------------


//------------------------------------------------------------------------------
// Public Member Functions
//------------------------------------------------------------------------------

void ServoGyro::calibrate(int inputPin, int outputPin) {
	calibrate(inputPin, outputPin, 250, 50);
}

// determine _minVoltage, _maxVoltage, _minPulseWidth, and _maxPulseWidth, i.e. the range of the servo
void ServoGyro::calibrate(int inputPin, int outputPin, int calibrationTimeStep, int calibrationBreakThreshold)  {
	// assumes that the inputPin is an analog pin and the outputPin a PWM digital pin

	float analogVoltagePrev = 0;
  float pwmWidthPrev = 0;
  int count = 0;
  int countThreshold = 10;
  int output = 1;

	if(this->attached()) {
		// move servo to approximately center position with generic pwm values
	 	this->writeMicroseconds((MIN_PULSE_WIDTH+MAX_PULSE_WIDTH)/2);
		delay(1500);

	  // find minimum position, analog voltage and pwm pulse width
    analogVoltagePrev = (float)analogRead(inputPin) + 5;
    pwmWidthPrev = (MIN_PULSE_WIDTH+MAX_PULSE_WIDTH)/2;
    count = 0;

		//while((abs((float)analogRead(inputPin) - (float)analogVoltagePrev)) > (float)0) {
		while(1) {
			if(output) {
				Serial.print("Previous Voltage / Current Voltage / Diff / PulseWidth: "); 
				Serial.print((float)analogRead(inputPin)); Serial.print(" / "); 
				Serial.print((float)analogVoltagePrev); Serial.print(" / "); 
				Serial.print(abs((float)analogRead(inputPin) - (float)analogVoltagePrev)); Serial.print(" / ");
				Serial.println(pwmWidthPrev);
			}
              
			analogVoltagePrev = (float)analogRead(inputPin);
			pwmWidthPrev = pwmWidthPrev - 15;
			this->writeMicroseconds(pwmWidthPrev);
			delay(calibrationTimeStep);

			if((abs((float)analogRead(inputPin) - (float)analogVoltagePrev)) > (float)calibrationBreakThreshold || pwmWidthPrev < MIN_PULSE_WIDTH && count < countThreshold) {
				count = count + 1;
				if(output) {
					Serial.print("Previous Voltage / Current Voltage / Diff / PulseWidth / Count: "); 
				  Serial.print((float)analogRead(inputPin)); Serial.print(" / "); 
				  Serial.print((float)analogVoltagePrev); Serial.print(" / "); 
				  Serial.print(abs((float)analogRead(inputPin) - (float)analogVoltagePrev)); Serial.print(" / ");
					Serial.print(pwmWidthPrev); Serial.print(" / "); 
					Serial.println(count); 
				}
				break;
			}
    }
      		
		_minVoltage = (int)analogVoltagePrev;
		_minPulseWidth = (int)pwmWidthPrev + 10;

		// move servo back to center position
	  this->writeMicroseconds((MIN_PULSE_WIDTH+MAX_PULSE_WIDTH)/2);
	  delay(1500);

		// find minimum position, analog voltage and pwm pulse width
    analogVoltagePrev = (float)analogRead(inputPin) - 5;
    pwmWidthPrev = (MIN_PULSE_WIDTH+MAX_PULSE_WIDTH)/2;
    count = 0;

	  //while((abs((float)analogRead(inputPin) - (float)analogVoltagePrev)) > (float)0) {
	  while(1) {
  		if(output) {
				Serial.print("Previous Voltage / Current Voltage / Diff / PulseWidth: "); 
		    Serial.print((float)analogRead(inputPin)); Serial.print(" / "); 
		    Serial.print((float)analogVoltagePrev); Serial.print(" / "); 
		    Serial.print(abs((float)analogRead(inputPin) - (float)analogVoltagePrev)); Serial.print(" / ");
				Serial.println(pwmWidthPrev);
			}
			
			analogVoltagePrev = (float)analogRead(inputPin);
		  pwmWidthPrev = pwmWidthPrev + 15;
		  this->writeMicroseconds(pwmWidthPrev);
		  delay(calibrationTimeStep);
		      
		  if((abs((float)analogRead(inputPin) - (float)analogVoltagePrev)) > (float)calibrationBreakThreshold || pwmWidthPrev > MAX_PULSE_WIDTH && count < countThreshold) {
				count = count + 1;
				if(output) {
					Serial.print("Previous Voltage / Current Voltage / Diff / PulseWidth: "); 
				  Serial.print((float)analogRead(inputPin)); Serial.print(" / "); 
				  Serial.print((float)analogVoltagePrev); Serial.print(" / "); 
				  Serial.print(abs((float)analogRead(inputPin) - (float)analogVoltagePrev)); Serial.print(" / ");
					Serial.print(pwmWidthPrev); Serial.print(" / "); 
					Serial.println(count); 
				}
		  	break;
		  }
	  }
      
		_maxVoltage = (int)analogVoltagePrev;
	  _maxPulseWidth = pwmWidthPrev - 10;

		if(output) {
			Serial.println("Servo Calibration Results");
			Serial.println("-------------------------");
			Serial.print("Minimum Voltage	:"); Serial.println(_minVoltage);
			Serial.print("Maximum Voltage	:"); Serial.println(_maxVoltage);
			Serial.print("Minimum PulseWidth:"); Serial.println(_minPulseWidth);
			Serial.print("Maximum PulseWidth:"); Serial.println(_maxPulseWidth);
		}
		
		_isCalibrated = 1;
	} else {
		if(output) {
			Serial.println("Error calibrate: Servo is not attached to an output pin!");
		}
	}
}

void ServoGyro::calibrateWithData(int minVoltage, int maxVoltage, int minPulseWidth, int maxPulseWidth) {
		int output = 1;
		
		_minVoltage = minVoltage;
		_minPulseWidth = maxVoltage;
		_maxVoltage = minPulseWidth;
	  _maxPulseWidth = maxPulseWidth;
	  
	  if(output) {
		 	Serial.println("Servo Calibration Results");
			Serial.println("-------------------------");
			Serial.print("Minimum Voltage	:"); Serial.println(_minVoltage);
			Serial.print("Maximum Voltage	:"); Serial.println(_maxVoltage);
			Serial.print("Minimum PulseWidth:"); Serial.println(_minPulseWidth);
			Serial.print("Maximum PulseWidth:"); Serial.println(_maxPulseWidth);
		}
		
		_isCalibrated = 1;
}

// move the servo arm to position specified by angle
void ServoGyro::writeAngle(int angle) {
	int tempAngle; 
	int pulseWidth;

	if(_isCalibrated) {
		// check if angle is in the servo range
		if(angle < _minAngle) {
			tempAngle = _minAngle;
			Serial.print("Warning writeAngle: Angle out of range, set to _minAngle: "); Serial.println(_minAngle);
		} else {
			tempAngle = angle;
		}

		if(angle > _maxAngle) {
			tempAngle = _maxAngle;
			Serial.print("Warning writeAngle: Angle out of range, set to _maxAngle: "); Serial.println(_maxAngle);
		} else {
			tempAngle = angle;
		}

		// map angle to pwm range and write it to servo	
		//map(value, fromLow, fromHigh, toLow, toHigh)
		pulseWidth = map(tempAngle, _minAngle, _maxAngle, _minPulseWidth, _maxPulseWidth);
		this->writeMicroseconds(pulseWidth);

		// record reference angle
		_refAngle = angle;
	} else {
		Serial.print("Error writeAngle: Servo is not calibrated");
	}
}

// returns the actual angle the servo is at
int ServoGyro::readAngle(int inputPin) {
	if(_isCalibrated) {
		// map read in voltage to an angle range
		return map((int)analogRead(inputPin), _minVoltage, _maxVoltage, _minAngle, _maxAngle);
	} else {
		return -1;
		Serial.println("Error readAngle: Servo is not calibrated.");
		Serial.println("		 Call the calibration function first.");
	}
}

// returns the _refAngle that the servo is trying reach
int ServoGyro::getAngle() {
	return _refAngle;
}

void ServoGyro::goToMin(int inputPin) {
	if(_isCalibrated) {
		//Serial.print("goToMin Minimum PulseWidth:"); Serial.println(_minPulseWidth);
		this->writeMicroseconds(_minPulseWidth);
	} else {
		Serial.println("Error goToMin: Servo is not calibrated.");
	}


}

void ServoGyro::goToMax(int inputPin) {
	if(_isCalibrated) {
		//Serial.print("goToMax Maximum PulseWidth:"); Serial.println(_maxPulseWidth);
		this->writeMicroseconds(_maxPulseWidth);
	} else {
		Serial.println("Error goToMax: Servo is not calibrated.");
	}
}

void ServoGyro::goToMiddle(int inputPin) {
	if(_isCalibrated) {
		this->writeMicroseconds((_minPulseWidth + _maxPulseWidth)/2);
	} else {
		Serial.println("Error goToMiddle: Servo is not calibrated.");
	}
}

int ServoGyro::getMinAngle() {
	return _minAngle;
}

int ServoGyro::getMaxAngle() {
	return _maxAngle;
}

int ServoGyro::getMinVoltage() {
	return _minVoltage;
}

int ServoGyro::getMaxVoltage() {
	return _maxVoltage;
}

int ServoGyro::getMinPulseWidth() {
	return _minPulseWidth;
}

int ServoGyro::getMaxPulseWidth() {
	return _maxPulseWidth;
}

//------------------------------------------------------------------------------
// Protected Member Functions
//------------------------------------------------------------------------------


//------------------------------------------------------------------------------
// Private Member Functions
//------------------------------------------------------------------------------

